
#include "HelperMover.h"
#include "Tangible.h"
#include "States.h"
#include <sstream>


void HelperMover::RotarYMover( int delta, Tangible * tan, Ogre::Vector3 const & objetivo )
{
	float deltaf = delta / 1000.0;

	NxOgre::Body * bod = tan->GetPhysBody();
	Ogre::SceneNode * nod = tan->GetNodo();

	Ogre::Vector3 const & curpos = bod->getGlobalPositionAsOgreVector3();

	
	// Rotamos hacia el objetivo.

	Ogre::Vector3 srcVec = bod->getGlobalOrientationAsOgreQuaternion() * Ogre::Vector3::UNIT_Z;
	srcVec.normalise();
	Ogre::Quaternion quat;

	Ogre::Vector3 objrot = objetivo - curpos;
	objrot.normalise();

	quat = srcVec.getRotationTo( objrot );

	Ogre::Degree p = quat.getPitch();
	Ogre::Degree r = quat.getRoll();
	Ogre::Degree y = quat.getYaw();

	Ogre::Vector3 torq(p.valueDegrees(), y.valueDegrees(), 
		r.valueDegrees());
	bod->setAngularVelocity(torq);



	// Avanzamos hacia el objetivo
	if(bod->getLinearVelocity().squaredLength() < 20000*20000)
		bod->addLocalForce(Ogre::Vector3(0, 0, 3000 * deltaf), NX_VELOCITY_CHANGE);


	//mLineaDebug1->beginUpdate(0);
	//mLineaDebug1->position(curpos);
	//mLineaDebug1->position(curpos + objrot * 1000);
	//mLineaDebug1->end();
}

HelperMover::HelperMover()
{	
	//bod->setMaxAngularVelocity(0.9);
	std::stringstream nom;
	static int nLineaDebug = 0;
	//nom << "LineaDebug" << nLineaDebug++;
	//mLineaDebug1 = GetMainGameState()->GetSceneManager()->
	//	createManualObject(nom.str().c_str());
	//mLineaDebug1->begin("red_laser_blast", Ogre::RenderOperation::OT_LINE_LIST);
	//mLineaDebug1->position(Ogre::Vector3(0, 0, 0));
	//mLineaDebug1->position(Ogre::Vector3(5000, 5000, 5000));
	//mLineaDebug1->end();
	//GetMainGameState()->GetSceneManager()->getRootSceneNode()->createChildSceneNode()->attachObject(mLineaDebug1);

}

bool HelperMover::CaminoDespejado( Tangible * tan, int distancia )
{
	Ogre::Vector3 adelante = 
		tan->GetPhysBody()->getGlobalOrientationAsOgreQuaternion() * 
		Ogre::Vector3::UNIT_Z;

	Ogre::Vector3 const & curpos = 
		tan->GetPhysBody()->getGlobalPositionAsOgreVector3();
	NxOgre::ActorRaycastHitNodeIterator actors = 
		tan->GetPhysScene()->getActorsFromRaycast(curpos, adelante, distancia);
	NxOgre::ActorRaycastHit * raycastHit;
	raycastHit = actors.Begin();


	bool despejado = true;
	while(raycastHit)
	{
		NxOgre::Actor * act = *raycastHit;
		if(act->getName() != tan->GetPhysBody()->getName())
		{
			despejado = false;
			break;
		}
		raycastHit = actors.Next();
	}

	return despejado;
}

bool HelperMover::DireccionDespejada( Tangible * tan, Ogre::Vector3 const & direccion, int distancia, bool local)
{
	Ogre::Vector3 direc;
	if(local)
		direc = 
		tan->GetPhysBody()->getGlobalOrientationAsOgreQuaternion() * direccion;
	else 
		direc = direccion;
	direc.normalise();

	Ogre::Vector3 curpos = 
		tan->GetPhysBody()->getGlobalPositionAsOgreVector3();
	curpos +=
		tan->GetPhysBody()->getGlobalOrientationAsOgreQuaternion() * 
		Ogre::Vector3(0, 0, -200);
	NxOgre::Scene * scene = tan->GetPhysScene();


	bool despejado = true;

	NxOgre::Raycaster * rayCaster = scene->createRaycaster();
	rayCaster->origin(curpos);
	rayCaster->normal(direc);
	rayCaster->distance(distancia);
	rayCaster->type(NxOgre::Raycaster::RCT_All);
	rayCaster->mScene = scene;
	rayCaster->accuracy(NxOgre::Raycaster::AC_Accurate);
	
	if (rayCaster->cast()) 
	{
		for (NxOgre::ActorRaycastHit * ar = rayCaster->mActors.Begin(); 
			ar = rayCaster->mActors.Next();) 
		{
			if(ar->mActor->getName() != tan->GetPhysBody()->getName())
			{
				despejado = false;
				break;
			}
		}
	}
	scene->destroyRaycaster(rayCaster);

	//mLineaDebug1->beginUpdate(0);
	//mLineaDebug1->position(curpos);
	//mLineaDebug1->position(curpos + direc * distancia);
	//mLineaDebug1->end();

	return despejado;
}

HelperMover::~HelperMover()
{
	//mLineaDebug1->setVisible(false);
	//GetMainGameState()->GetSceneManager()->destroyManualObject(mLineaDebug1);
}